"""skytank_utils.py Utilities for the SkyTanx robotics platform

This program contains functions for interfacing with SkyTanx
- Has dependencies in Skytank_image.py

"""

__author__ = "Leo Szeto"
__verion__ = "$Revision: 0.1 $"
__date__ = "$Date: 2011/02/17 $"
__copyright__ = "Copyright (c) 2011 Leo Szeto"
__license__ = "Python"

import sys, math, time, string, os

#Import wrapper libraries
import Skype4Py #Netbook -> Internet layer
#import wave     #Netbook -> Sound
import serial   #Netbook -> Circuits layer
import Skytank_mutex
from Skytank_mutex import *
import Skytank_image
from Skytank_image import *
#Create an instance of the Skype class
skype = Skype4Py.Skype()
imaging_mode = 0 	 #If this is 1, then the command is set to imaging mode.
in_call      = 0	 #If this is 1, then the robot is in a call
#Create an instance of the serial class, go through ttyUSB0 to ttyUSB4 
port = 0
while(port < 5):
	try:
		print 'Contacting /dev/ttyUSB' + str(port) + '....'
		ser = serial.Serial('/dev/ttyUSB' + str(port))
		break
	except serial.serialutil.SerialException:
		print "Invalid Serial port!"
		port = port + 1

#Close down the script if no port is found at all
if(port == 5):
	print "No serial port found!"
	sys.exit()
else:
	print 'Connnected to serial port ttyUSB' + str(port)
#Create an instance of the mutex lock
#SkytankMutex lock;

#Custom Functions
#-----------------------------------------------------------------------------------	
#SERIAL EVENT HANDLERS
#------de-----------------------------------------------------------------------------	
#Handshake function
#Attempts to complete a handshake with a device
#Returns the device signature if success, returns -1 if it fails.
def handshake(ser, max_tries, timeout):
	max_tries = math.fabs(max_tries) 			#Makes sure that the attempts value is positive
	tries = 0
	command = "FAILLINK"					#Defaults to a communication failure
	print "Sending info"
	ser.write("INIT0000") 					#Send the INIT command to the arduino
	while tries < max_tries:
		print "Attempt #" + str(tries) + "..."
		if ser.inWaiting() >= 8: 			#Wait until there are 8 bytes (At least 1 cmd)
			print "Command received"
			command = ser.read(8)   		#Read the cmd (8 bytes)
			tries = max_tries			#We are now done, exit
		else:
			tries = tries + 1
		time.sleep(timeout)				#timeout for a certain time
	return command 				 		#Return the results back

#-----------------------------------------------------------------------------------	
#Listening function
#Simple function to check if any commands are in the pipeline. If so, parse it.
def serial_listen(ser):
	command = ""
	if ser.inWaiting() >= 8:				#We have at least 1 command in the pipeline
		command = ser.read(8)   			#Read the cmd (8 bytes) 
	return command						#Return result

#-----------------------------------------------------------------------------------	
#Move function
#Attempts to move the vehicle. left and right denote the left and right motors
#and has a range of 0 - 255. 0 - 127 is backwards while 128 - 255 is forwards.
#Returns 0 if arduino confirms, returns -1 if it fails
def move_car(left, right, delay):
	#we need to convert left and right into binary values
	left = int(left)
	right = int(right)
	delay = int(delay)
	
	left = str(left)
	right = str(right)
	delay = str(delay)
	return "MOVE0" + left + right + delay

#-----------------------------------------------------------------------------------	
#LED control
#Attempts to turn on an LED. value denotes the LED that is toggled.
#Returns 0 if arduino confirms, returns -1 if it fails
def LED_ctrl(value):
	return 0

#-----------------------------------------------------------------------------------	
#Request ID
#Requests for the UID of the arduino. This is used to figure out which USB port has
#which arduino. Returns 0 if arduino confirms, returns -1 if it fails
def req_ID():
	return "RQID0000"

def about_message(instr, num_items):
	return "This is about SkyTanX"

def help_message(instr, num_items):
	#First item is help, second item is command. 
	if num_items > 0:
		help_cmd = string.upper(instr[1]) #All upper case
		if help_cmd == "MOVE":
			skype_string = "Explaination for MOVE\n"
		elif help_cmd == "HALT":
			skype_string = "Explaination for HALT\n"
		elif help_cmd == "LEDCONTROL":
			skype_string = "Explaination for LED Control\n"
		elif help_cmd == "HELP":
			skype_string = "Don't be silly.\n"
	#Otherwise, do a general listing of commands
	else: 
		skype_string = "SKYTANX Commands: \n\
		type in HELP [command] for more information \n\
		MOVE: 		Moves the robot. (L/R/D) \n\
		HALT: 		Stops the robot immediately. \n\
		LEDCONTROL: 	Controls the LEDS on robot.\n"
	return skype_string

#-----------------------------------------------------------------------------------	
#Sends the command down stream
def serial_execute(cmd):
	ser.write(cmd) 
	return 0

#-----------------------------------------------------------------------------------	
#SKYPE EVENT HANDLERS
#-----------------------------------------------------------------------------------
#Translates the instruction into a arduino compatible instruction
def instr_decode(instr, Message, num_items):
	global imaging_mode #Declares for imaging mode to be edited on the global variable
	print instr
	#Here, we need to parse the instruction out to a serial based instruction
	#Start by parsing the instruction.
	serial_string = ""
	command = instr[0]
	#Normalize the instruction
	command = string.upper(command) #All upper case
	#Check for commands
	if command == "MOVE":		#Start some movement
		print "Command received: Move"
		#Now we must convert the parameters out to a signal
		serial_string = move_car(instr[1], instr[2], instr[3]) #L/R/Delay
		print serial_string
	elif command == "HALT":		#Stop all movements
		print "HALT!"
		serial_string = move_car(0, 0, 0) #NOT TESTED
	elif command == "LEDCONTROL":
		print "Command received: LED Control"
		serial_string = "LEDC0000"
	elif command == "HELP":
		skype.CreateChatWith(Message.FromHandle).SendMessage(help_message(instr, num_items))
	elif command == "ABOUT":
		skype.CreateChatWith(Message.FromHandle).SendMessage(about_message(instr, num_items))
	elif command == "HELLO":
		skype.CreateChatWith(Message.FromHandle).SendMessage("Hello there! I'm SkytanX!")
	elif command == "DANCE":
		skype.CreateChatWith(Message.FromHandle).SendMessage("I love to dance!")
		serial_string = move_car(3, 9, 8)
	elif command == "BYE":
		skype.CreateChatWith(Message.FromHandle).SendMessage("Goodbye! It was fun!")	
	elif command == "PICTURE": #Tester command to take a picture of the desktop
		print "Here's a picture!"
		pb = Skytank_image.grab_image(window, 60, 80, 300, 300)
		im = Skytank_image.translate_to_PIL(pb, 300, 300)
		im.show()
		skype.CreateChatWith(Message.FromHandle).SendMessage("Picture Taken!")	
	elif command == "IMAGING": #Beta test for the imaging mode
		print "Imaging mode: Type in EXIT to quit imaging mode"
		skype.CreateChatWith(Message.FromHandle).SendMessage("Starting imaging mode (beta). Use a red object to control movement.")
		imaging_mode = 1
		print "instr_decode: " + str(imaging_mode)
	elif command == 'EXIT':
		imaging_mode = 0
		print "Closing imaging mode."
		print "instr_decode: " + str(imaging_mode)		
		skype.CreateChatWith(Message.FromHandle).SendMessage("Closing connection.")	
	else:
		serial_string = "FAILCODE"
	return serial_string
	
#Translates the messages received from skype to serial messages
#Multiple instructions can be parsed and 
def CmdTranslate(instructions, Message):
	for instruction in instructions.split(";"):
		instruction = string.strip(instruction)  #Trim the leading spaces
		arguments = instruction.split(" ")	#Split the Messages into a list of parameters
		#Find the number of items we have in the instruction
		num_items = -1
		for args in arguments:
			num_items = num_items + 1
		#Send it to instruction decode to transfer message to serial
		serial_cmd = instr_decode(arguments, Message, num_items)
		print "CmdTranslate: " + str(imaging_mode)
		if serial_cmd == "FAILCODE":
			skype.CreateChatWith(Message.FromHandle).SendMessage("Error: Incorrect command (" + ' '.join(arguments) + "), type help for more info")
	return serial_cmd
	
def OnAttach(status): 
	print 'API attachment status: ' + skype.Convert.AttachmentStatusToText(status)
	if status == Skype4Py.apiAttachAvailable:
		skype.Attach();
	if status == Skype4Py.apiAttachSuccess:
	   print('Skype connected successfully'); 
	   
#-------------------------------------------------------------------------------------
# Fired on chat message status change. 
# Statuses can be: 'UNKNOWN' 'SENDING' 'SENT' 'RECEIVED' 'READ'		

#We received a message!
def OnMessageStatus(Message, Status): 
	if Status == 'RECEIVED':
		#Here is where the magic happens. It is the entry point for any and all
		#communications through Skype into the python script. We will be filtering
		#to see if the message is a valid command from a valid user, and then we
		#will send this command down the translation layer which will send it to
		#the arduino.
		print(Message.FromDisplayName + ': ' + Message.Body)
		#ser.write("MOVE0000")
		#Check if the robot is locked by a user.
		# - A lock occurs when a user is in a call with another user
		# - A user has sent the robot a message in the last 30 seconds
		
		#Check if the sender is an authorized user.
		# - If the sender is authorized and the robot isn't locked, give control to
		#   the user
		
		#Sanity check the sent command to see if they are legitimate. If not, send back
		#an error message.
		# - Do they start with #?
		# - Are they part of the instruction list?
		# - Do they have enough parameters?
		
		#Once these are cleared, then send the message down the translation function.
		Cmd = CmdTranslate(Message.Body, Message)
		print "Sending command to arduino: " + Cmd
		#Use the command now.
		#We must first achieve a lock first in order to ensure proper commands being sent.
		#Python synchronization exists, see: (effbot.org/zone/thread-synchronization.htm)
		serial_execute(Cmd)
	
	if Status == 'SENT':
		print('Sent back: ' + Message.Body)

#------------------------------------------------------------------------------------
#Event handler that pings the script whenever skype receives a call.
#Statuses (NOT ALL): NOT_AVAILABLE, FAILED, RINGING, INPROGRESS, ONHOLD, FINISHED, MISSED, REFUSED, BUSY, CANCELLED
#(Need to build a tester) to know what triggers what
def OnCall(call, status):
	global in_call	#Set in_call flag to global variable
	if status == 'RINGING':
		print "I gotta feeling, and it's a call!"
		in_call = 1
	elif status == 'FINISHED':
		in_call = 0
		print "That was fun! (Ended Call)"
	elif status == 'INPROGRESS':
		print "Uh oh! Someone tried to call when I'm already in a call!"
	#We need to reject any users if we are already in a call
	print "status is ", status, " Peer is: ", call.PartnerHandle, " Show name is ", call.PartnerDisplayName
	#Pick up the call if it's in the call state
	if in_call == 1:
		call.Answer()
	return 0
	
def process_image(image, color):	
	#print str(imaging_mode) + str(in_call)
	if imaging_mode == 1 and in_call == 1:
		im = Skytank_image.translate_to_PIL(image.capture(), 300, 300)		
		#Measure the filter time		
		#start = time.time()		
		out = filter_module(im, color, 70, 9, (300, 300), 0.3)
		stat = get_bbox_stats(out)
		#print "Time: " + str(time.time() - start)
		print stat
		return stat
	return -1
		
#For now, we only use one color so there is only 1 command
def process_command(stat):
	#print "Processing image command"
	print stat
	#For now, let's have a binary system where it either moves or doesn't
	x_off = stat[0] - 45
	y_off = stat[1] - 45
	#Figure out the mag and theta
	mag = math.sqrt(math.pow(x_off, 2) + math.pow(y_off, 2))
	angle = math.atan2(y_off, x_off)
	#Figure out the L and R commands based on the the follow equations
	#L = 4.5(sin(@)+cos(@)+1)
	#R = 4.5(sin(@)-cos(@)+1)
	Left  = 4.5*(math.sin(angle) + math.cos(angle) + 1)
	Right = 4.5*(math.sin(angle) - math.cos(angle) + 1)
	#Output calculations
	print 'Magnitude: ' + str(mag) + ' Angle: ' + str(angle) + ' Left: ' + str(Left) + ' Right: ' + str(Right)
	if mag > 10:
		serial_execute("MOVE0990")
		print "Moving Command!"
	else:
		serial_execute("HALT0000")
